#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "trajectory_msgs/msg/joint_trajectory.hpp"
#include "sensor_msgs/msg/joint_state.hpp"

using std::placeholders::_1;

class JointStateListener : public rclcpp::Node
{
public:
    JointStateListener() : Node("listener")
    {
        subscription_ = this->create_subscription<sensor_msgs::msg::JointState>(
            "/joint_states", 10, std::bind(&JointStateListener::callback, this, _1));
    }

private:
    void callback(const sensor_msgs::msg::JointState::SharedPtr msg)
    {
        float pos[5] = {0};
        int len = msg->position.size();

        for (int j = 0; j < len && j < 5; j++) {
            pos[j] = msg->position[j];
        }

        if (len >= 5) {
            RCLCPP_INFO(this->get_logger(), "pos, %f, %f, %f, %f, %f", 
                        pos[0], pos[1], pos[2], pos[3], pos[4]);
        }
    }

    rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr subscription_;
};

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<JointStateListener>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

// // 实时发送关节角度信息
// #include "rclcpp/rclcpp.hpp"
// #include "std_msgs/String.h"
// #include "trajectory_msgs/JointTrajectory.h"
// #include "sensor_msgs/JointState.h"

// void Callback(const sensor_msgs::JointStateConstPtr& msg)
// {
//     float pos[5] = {0};
//     int len = msg->position.size();  // 一段轨迹有多少个路径点
//     for (int j = 0; j < len && j < 5; j++) {  // 确保不会越界
//         pos[j] = msg->position[j];
//     }
//     if (len >= 5) {  // 只有当我们确实有5个或更多的位置数据时，才打印信息
//         ROS_INFO("pos,%f,%f,%f,%f,%f", pos[0], pos[1], pos[2], pos[3], pos[4]);
//     }
// }

// int main(int argc, char** argv)
// {
//     ros::init(argc, argv, "listener"); //listener 
//     ros::NodeHandle n;
//     ros::Subscriber sub = n.subscribe("/joint_states", 10000, Callback); //
//     ros::Rate rate(100);
//     ros::spin();
//     return 0;
// }

